#ifndef _MOTOR_H_
#define _MOTOR_H_
#include <Arduino.h>
#include "distance.h"
#include "gray_sensor.h"
class motor_cs
{
private:
    const int rmotor_pin1 = 3;
    const int rmotor_pin2 = 5;
    const int lmotor_pin1 = 6;
    const int lmotor_pin2 = 9;
    int left_pwm = 150;
    int right_pwm = 140;

public:
    typedef enum
    {
        speed_long = 0,
        speed_left,
        speed_right,
        speed_first,
        speed_block,
        speed_stop,
    } speed_status;
    speed_status speed_flag = speed_first;
    motor_cs()
    {
        pinMode(lmotor_pin1, OUTPUT);
        pinMode(lmotor_pin2, OUTPUT);

        pinMode(rmotor_pin1, OUTPUT);
        pinMode(rmotor_pin2, OUTPUT);
    };
    void motor_control(int sensor1, int sensor2, float distance)
    {
        Serial.println(speed_flag);
        // float distance = distance_date.get_distance();
        if (speed_flag != speed_stop)
            if (distance >= 10.00 && speed_flag != speed_block || speed_flag == speed_first)
            {
                if (speed_flag == speed_long || speed_flag == speed_first)
                {

                    if (sensor1 < 200 && sensor2 > 450)
                    {
                        analogWrite(rmotor_pin1, 240);
                        digitalWrite(rmotor_pin2, 0);
                        analogWrite(lmotor_pin1, left_pwm - 80);
                        digitalWrite(lmotor_pin2, 0);
                        speed_flag = speed_right;
                    }

                    else if (sensor2 < 300 && sensor1 > 320)
                    {
                        analogWrite(lmotor_pin1, 240);
                        digitalWrite(lmotor_pin2, 0);
                        analogWrite(rmotor_pin1, right_pwm - 80);
                        digitalWrite(rmotor_pin2, 0);
                        speed_flag = speed_left;
                    }
                    else
                    {
                        speed_flag = speed_long;
                        analogWrite(lmotor_pin1, left_pwm);
                        digitalWrite(lmotor_pin2, 0);
                        analogWrite(rmotor_pin1, right_pwm);
                        digitalWrite(rmotor_pin2, 0);
                    }
                }
                if (speed_flag != speed_long)
                    if (sensor2 > 350 && sensor1 > 350)
                    {
                        speed_flag = speed_long;
                        analogWrite(lmotor_pin1, left_pwm);
                        digitalWrite(lmotor_pin2, 0);
                        analogWrite(rmotor_pin1, right_pwm);
                        digitalWrite(rmotor_pin2, 0);
                    }
            }
            else if (distance < 10 && speed_flag != speed_first && distance >= 1)
            {
                digitalWrite(lmotor_pin1, 0);
                digitalWrite(lmotor_pin2, 230);
                digitalWrite(rmotor_pin1, 0);
                digitalWrite(rmotor_pin2, 230);
                speed_flag = speed_block;
            }
            else if (speed_flag == speed_block && distance >= 20.0)
            {
                speed_flag = speed_stop;
                digitalWrite(lmotor_pin1, 0);
                digitalWrite(lmotor_pin2, 0);
                digitalWrite(rmotor_pin1, 0);
                digitalWrite(rmotor_pin2, 0);
            }
    };
};
#endif